home *** CD-ROM | disk | FTP | other *** search
/ HPAVC / HPAVC CD-ROM.iso / pc / 3DTOSHI2.ZIP / mpg3d / source / G3dmath.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1996-04-20  |  7.7 KB  |  247 lines
  1.  
  2. // g3dmath.cpp
  3. //
  4. // Copyright (c) 1995 by Toshiaki Tsuji, all rights reserved.
  5.  
  6. #include "stdgfx.h"
  7. #include "g3dmath.h"
  8. #include "lg3dmat.h"
  9. #include <math.h>
  10.  
  11. //*************************************************
  12. //
  13. // Matrix Operations
  14. //
  15. //*************************************************
  16.  
  17. VOID InitMatrix ( G3DMATRIX Matrix )
  18.   {
  19.     INT i,j;
  20.     for (i=0;i<4;i++)
  21.       {
  22.         for (j=0;j<4;j++)
  23.           {
  24.             if (i==j)
  25.               Matrix[i][j] = (float)1;
  26.             else    
  27.               Matrix[i][j] = (float)0;
  28.           } // End for  
  29.       } // End for  
  30.   } // End for InitMatrix
  31.  
  32. VOID CopyMatrix ( G3DMATRIX Src, G3DMATRIX Dest )
  33.   {
  34.     memcpy ( Dest, Src, 4*4*4 );
  35.   } // End for CopyMatrix
  36.  
  37. VOID MultMatrix ( G3DMATRIX Mat1, G3DMATRIX Mat2, G3DMATRIX Dest )
  38.   {
  39.     _FLPMultMatrix ( Mat1, Mat2, Dest ); 
  40.   } // End for MultMatrix
  41.  
  42. VOID Translate ( G3DMATRIX Matrix, float xt, float yt, float zt )
  43.   {
  44.     G3DMATRIX TMat;
  45.     G3DMATRIX Mat1;
  46.     float a;
  47.  
  48.     a = (float)1;
  49.  
  50.     TMat[0][0]=  (float)a; TMat[0][1]=  (float)0; TMat[0][2]=  (float)0; TMat[0][3]=  (float)0;
  51.     TMat[1][0]=  (float)0; TMat[1][1]=  (float)a; TMat[1][2]=  (float)0; TMat[1][3]=  (float)0;
  52.     TMat[2][0]=  (float)0; TMat[2][1]=  (float)0; TMat[2][2]=  (float)a; TMat[2][3]=  (float)0;
  53.     TMat[3][0]=  (float)xt; TMat[3][1]=  (float)yt; TMat[3][2]=  (float)zt; TMat[3][3]=  (float)a;
  54.  
  55.     MultMatrix ( Matrix, TMat, Mat1 );
  56.     CopyMatrix ( Mat1, Matrix ); 
  57.   } // End for Translate
  58.  
  59. VOID Scale ( G3DMATRIX Matrix, float ScaleFactor )
  60.   {
  61.     G3DMATRIX SMat;
  62.     G3DMATRIX Mat1;
  63.     float a;
  64.     float s = ScaleFactor;
  65.  
  66.     a = (float)1;
  67.  
  68.     SMat[0][0]=  (float)s; SMat[0][1]=  (float)0; SMat[0][2]=  (float)0; SMat[0][3]=  (float)0;
  69.     SMat[1][0]=  (float)0; SMat[1][1]=  (float)s; SMat[1][2]=  (float)0; SMat[1][3]=  (float)0;
  70.     SMat[2][0]=  (float)0; SMat[2][1]=  (float)0; SMat[2][2]=  (float)s; SMat[2][3]=  (float)0;
  71.     SMat[3][0]=  (float)0; SMat[3][1]=  (float)0; SMat[3][2]=  (float)0; SMat[3][3]=  (float)a;
  72.  
  73.     MultMatrix ( Matrix, SMat, Mat1 );
  74.     CopyMatrix ( Mat1, Matrix ); 
  75.   } // End of Scale 
  76.  
  77. VOID RotateXYZ ( G3DMATRIX Matrix, float xa, float ya, float za )
  78.   {
  79.     G3DMATRIX XMat,YMat,ZMat,Mat1,Mat2;
  80.     double Xa,Ya,Za;
  81.     float Sx,Cx,Sy,Cy,Sz,Cz;
  82.  
  83.     // Change degree to radian
  84.     Xa = -xa*6.281/360;
  85.     Ya = -ya*6.281/360;
  86.     Za = -za*6.281/360;
  87.     
  88.     Sx = (float)sin ( Xa );
  89.     Cx = (float)cos ( Xa );
  90.     Sy = (float)sin ( Ya );
  91.     Cy = (float)cos ( Ya );
  92.     Sz = (float)sin ( Za );
  93.     Cz = (float)cos ( Za );
  94.  
  95.     float a;
  96.  
  97.     a = (float)1;
  98.     XMat[0][0]=  (float)a; XMat[0][1]=  (float)0; XMat[0][2]=  (float)0; XMat[0][3]=  (float)0;
  99.     XMat[1][0]=  (float)0; XMat[1][1]= (float)Cx; XMat[1][2]= (float)Sx; XMat[1][3]=  (float)0;
  100.     XMat[2][0]=  (float)0; XMat[2][1]=(float)-Sx; XMat[2][2]= (float)Cx; XMat[2][3]=  (float)0;
  101.     XMat[3][0]=  (float)0; XMat[3][1]=  (float)0; XMat[3][2]=  (float)0; XMat[3][3]=  (float)a;
  102.     
  103.     YMat[0][0]= (float)Cy; YMat[0][1]=  (float)0; YMat[0][2]=(float)-Sy; YMat[0][3]=  (float)0;
  104.     YMat[1][0]=  (float)0; YMat[1][1]=  (float)a; YMat[1][2]=  (float)0; YMat[1][3]=  (float)0;
  105.     YMat[2][0]= (float)Sy; YMat[2][1]=  (float)0; YMat[2][2]= (float)Cy; YMat[2][3]=  (float)0;
  106.     YMat[3][0]=  (float)0; YMat[3][1]=  (float)0; YMat[3][2]=  (float)0; YMat[3][3]=  (float)a;
  107.     
  108.     ZMat[0][0]= (float)Cz; ZMat[0][1]= (float)Sz; ZMat[0][2]=  (float)0; ZMat[0][3]=  (float)0;
  109.     ZMat[1][0]=(float)-Sz; ZMat[1][1]= (float)Cz; ZMat[1][2]=  (float)0; ZMat[1][3]=  (float)0;
  110.     ZMat[2][0]=  (float)0; ZMat[2][1]=  (float)0; ZMat[2][2]=  (float)a; ZMat[2][3]=  (float)0;
  111.     ZMat[3][0]=  (float)0; ZMat[3][1]=  (float)0; ZMat[3][2]=  (float)0; ZMat[3][3]=  (float)a;
  112.  
  113.     MultMatrix ( Matrix, XMat, Mat1 );
  114.     MultMatrix ( Mat1, YMat, Mat2 );
  115.     MultMatrix ( Mat2, ZMat, Matrix );
  116.   } // End for RotateXYZ
  117.  
  118. VOID RotateYXZ ( G3DMATRIX Matrix, float xa, float ya, float za )
  119.   {
  120.     G3DMATRIX XMat,YMat,ZMat,Mat1,Mat2;
  121.     double Xa,Ya,Za;
  122.     float Sx,Cx,Sy,Cy,Sz,Cz;
  123.  
  124.     // Change degree to radian
  125.     Xa = -xa*6.281/360;
  126.     Ya = -ya*6.281/360;
  127.     Za = -za*6.281/360;
  128.     
  129.     Sx = (float)sin ( Xa );
  130.     Cx = (float)cos ( Xa );
  131.     Sy = (float)sin ( Ya );
  132.     Cy = (float)cos ( Ya );
  133.     Sz = (float)sin ( Za );
  134.     Cz = (float)cos ( Za );
  135.  
  136.     float a;
  137.  
  138.     a = (float)1;
  139.     XMat[0][0]=  (float)a; XMat[0][1]=  (float)0; XMat[0][2]=  (float)0; XMat[0][3]=  (float)0;
  140.     XMat[1][0]=  (float)0; XMat[1][1]= (float)Cx; XMat[1][2]= (float)Sx; XMat[1][3]=  (float)0;
  141.     XMat[2][0]=  (float)0; XMat[2][1]=(float)-Sx; XMat[2][2]= (float)Cx; XMat[2][3]=  (float)0;
  142.     XMat[3][0]=  (float)0; XMat[3][1]=  (float)0; XMat[3][2]=  (float)0; XMat[3][3]=  (float)a;
  143.     
  144.     YMat[0][0]= (float)Cy; YMat[0][1]=  (float)0; YMat[0][2]=(float)-Sy; YMat[0][3]=  (float)0;
  145.     YMat[1][0]=  (float)0; YMat[1][1]=  (float)a; YMat[1][2]=  (float)0; YMat[1][3]=  (float)0;
  146.     YMat[2][0]= (float)Sy; YMat[2][1]=  (float)0; YMat[2][2]= (float)Cy; YMat[2][3]=  (float)0;
  147.     YMat[3][0]=  (float)0; YMat[3][1]=  (float)0; YMat[3][2]=  (float)0; YMat[3][3]=  (float)a;
  148.     
  149.     ZMat[0][0]= (float)Cz; ZMat[0][1]= (float)Sz; ZMat[0][2]=  (float)0; ZMat[0][3]=  (float)0;
  150.     ZMat[1][0]=(float)-Sz; ZMat[1][1]= (float)Cz; ZMat[1][2]=  (float)0; ZMat[1][3]=  (float)0;
  151.     ZMat[2][0]=  (float)0; ZMat[2][1]=  (float)0; ZMat[2][2]=  (float)a; ZMat[2][3]=  (float)0;
  152.     ZMat[3][0]=  (float)0; ZMat[3][1]=  (float)0; ZMat[3][2]=  (float)0; ZMat[3][3]=  (float)a;
  153.  
  154.     MultMatrix ( Matrix, YMat, Mat1 );
  155.     MultMatrix ( Mat1, XMat, Mat2 );
  156.     MultMatrix ( Mat2, ZMat, Matrix );
  157.   } // End for RotateYXZ
  158.  
  159.  
  160. //*************************************************
  161. //
  162. // Vector Operations
  163. //
  164. //*************************************************
  165.  
  166. float FLPVectorDot ( FLPVECTOR3D *V1, FLPVECTOR3D *V2 )
  167.   {
  168.     float Product;
  169.     Product = V1->x*V2->x + V1->y*V2->y + V1->z*V2->z;
  170.     return Product;
  171.   } // End of FLPVectorDot 
  172.  
  173. float FLPVectorDistance ( FLPVECTOR3D *P1, FLPVECTOR3D *P2 )
  174.   {
  175.     float xd,yd,zd;
  176.     float Distance;
  177.     
  178.     xd = P2->x - P1->x;  
  179.     yd = P2->y - P1->y;  
  180.     zd = P2->z - P1->z;
  181.  
  182.     Distance = xd*xd + yd*yd + zd*zd;
  183.     Distance = (float)sqrt ( Distance );
  184.     return Distance;
  185.   } // End of FLPVectorDistance
  186.  
  187. float FLPVectorMagnitude ( FLPVECTOR3D *V1 )
  188.   {
  189.     float Mag;
  190.     
  191.     Mag = V1->x*V1->x + V1->y*V1->y + V1->z*V1->z;
  192.     Mag = (float)sqrt ( Mag );
  193.     return Mag;
  194.   } // End of FLPVectorMagnitude
  195.  
  196. VOID FLPVectorAdd ( FLPVECTOR3D *V1, FLPVECTOR3D *V2, FLPVECTOR3D *Result )
  197.   {
  198.     Result->x = V2->x + V1->x;  
  199.     Result->y = V2->y + V1->y;  
  200.     Result->z = V2->z + V1->z;  
  201.   } // End of FLPVectorAdd 
  202.  
  203. VOID FLPVectorSub ( FLPVECTOR3D *V1, FLPVECTOR3D *V2, FLPVECTOR3D *Result )
  204.   {
  205.     Result->x = V2->x - V1->x;  
  206.     Result->y = V2->y - V1->y;  
  207.     Result->z = V2->z - V1->z;  
  208.   } // End of FLPVectorSub
  209.  
  210. VOID FLPVectorNormalize ( FLPVECTOR3D *V1 )
  211.   {
  212.     float Mag;
  213.     float Ratio;
  214.     
  215.     Mag = FLPVectorMagnitude ( V1 );
  216.     if (Mag==0)
  217.       return;
  218.       
  219.     Ratio = (float)1/Mag;
  220.     V1->x *= Ratio;  
  221.     V1->y *= Ratio;  
  222.     V1->z *= Ratio;  
  223.   } // End of FLPVectorNormalize 
  224.  
  225. VOID FLPVectorCross ( FLPVECTOR3D *V1, FLPVECTOR3D *V2, FLPVECTOR3D *Result )
  226.   {
  227.     float x1,y1,z1;  
  228.     float x2,y2,z2;
  229.  
  230.     x1 = V1->x;
  231.     y1 = V1->y;
  232.     z1 = V1->z;
  233.     
  234.     x2 = V2->x;
  235.     y2 = V2->y;
  236.     z2 = V2->z;
  237.     
  238.     Result->x = (y1)*(z2) - (z1)*(y2);
  239.     Result->y = - ((x1)*(z2) - (z1)*(x2));
  240.     Result->z = (x1)*(y2) - (y1)*(x2);
  241.   } // End of FLPVectorCross 
  242.  
  243.  
  244.  
  245.  
  246.  
  247.